//Frontier detection
#include "ros/ros.h"
#include "ros/console.h"
#include "geometry_msgs/Twist.h"
#include "sensor_msgs/LaserScan.h"
#include <cstdlib> // Needed for rand()
#include <ctime> // Needed to seed random number generator with a time value
#include <math.h>

#include "nav_msgs/OccupancyGrid.h"
#include "nav_msgs/GetMap.h"
#include "nav_msgs/Path.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseArray.h"

#include <vector>
#include <deque>
#include <set>
#include <utility>
#include "tf/transform_listener.h"   
#include "turtlebot_frontier/FSRequest.h"
#include "turtlebot_frontier/VantagePointArray.h"


class FrontierDetector {
public:
    bool scanFrontiers(turtlebot_frontier::FSRequest::Request &req, turtlebot_frontier::FSRequest::Response response);

    FrontierDetector(ros::NodeHandle& nh) {
        srand(time(NULL)); //Initialize a random generator
		
		//Initialize publishers
        frontierPub = nh.advertise<nav_msgs::OccupancyGrid>("frontier", 1);
        vantagePointPub = nh.advertise<nav_msgs::OccupancyGrid>("vantage", 1);
		vantagePointArrayPub = nh.advertise<turtlebot_frontier::VantagePointArray>("vantage_points", 1);
		//Initialize service client and map subscriber
        mapClient = nh.serviceClient<nav_msgs::GetMap>("dynamic_map");
        mapSubscriber = nh.subscribe<nav_msgs::OccupancyGrid>("map", 1, &FrontierDetector::mapSubscriberCallback, this);

    }

    void mapSubscriberCallback(nav_msgs::OccupancyGrid newMap) {
        map = newMap;
		//Store the map
    }

    bool scanFrontiers(turtlebot_frontier::FSRequest::Request &req, turtlebot_frontier::FSRequest::Response &response) {
        ROS_INFO("scanFrontiers started");
        nav_msgs::GetMap srv_map;
		//Try calling the get_map service in case the map server requires a service call
        if(mapClient.call(srv_map)) {
            if(srv_map.response.map.data.size() > 0) map = srv_map.response.map;
        }
        if(map.data.size() > 0) {
            //Set up the laser mask if necessary
            if(laserMask.size() == 0) {
                laserMaskSize = 2*ceil(laserRange / map.info.resolution) + 1;
                laserMaskOffset = ceil(laserRange / map.info.resolution) + 1;

                unsigned int r2 = ceil(laserRange / map.info.resolution) * ceil(laserRange / map.info.resolution);
                //unsigned int mr2 = ceil(laserMinRange / map.info.resolution) * ceil(laserMinRange / map.info.resolution);

                for(int x=0; x<laserMaskSize; x++) {
                    std::deque<bool> column;
                    for(int y=0; y<laserMaskSize; y++) {
                        if((x-laserMaskOffset) * (x-laserMaskOffset) + (y-laserMaskOffset)*(y-laserMaskOffset) <= r2 /*&& (x-laserMaskOffset) * (x-laserMaskOffset) + (y-laserMaskOffset)*(y-laserMaskOffset) >= mr2*/) {
                            column.push_back(true);
                        }
                        else {
                            column.push_back(false);
                        }
                    }
                    laserMask.push_back(column);
                }


                std::cout << "Laser mask size: " << laserMaskSize << std::endl;
            }
        }
        else {
            ROS_ERROR("No map stored, quitting.");
			return false;
        }

		//vantageMap stores the amount of frontier nodes visible from a node
        nav_msgs::OccupancyGrid vantageMap;
        vantageMap.header.stamp = ros::Time::now();
        vantageMap.header.frame_id = map.header.frame_id;
        vantageMap.info = map.info;
        vantageMap.data.resize(map.data.size());
        for(unsigned long int i=0; i<vantageMap.data.size(); i++) {
            vantageMap.data[i] = 0;
        }

		//These arrays are easier to handle than occupancy grids
        std::vector<std::vector<int> > mapArray; //Stores the map
        std::vector<std::vector<int> > vantageArray; //Stores vantage points
        std::vector<std::deque<bool> > frontierArray; //Stores frontiers
        std::vector<std::vector<int> > wallArray; //Stores walls only

		//The algorithm will be restricted to the actually used parts of the map
        int minX = map.info.width-1;
        int maxX = 0;
        int minY = map.info.height-1;
        int maxY = 0;

        //Convert the map into a more usable instance, initialize the other arrays
        for(unsigned int x=0; x<map.info.width; x++) {
            std::vector<int> column;
            std::vector<int> vantageColumn;
            std::deque<bool> frontierColumn;
            std::vector<int> wallColumn;
            for(unsigned int y=0; y<map.info.height; y++) {
                if(map.data[y*map.info.width + x] > 25) {
                    column.push_back(INT_MAX);
                    vantageColumn.push_back(-1);
                    wallColumn.push_back(1);
                }
                else {
                    if(map.data[y*map.info.width + x] >= 0) {
                        column.push_back(0);
                        vantageColumn.push_back(0);
                        if(x > maxX) maxX = x;
                        if(x < minX) minX = x;
                        if(y > maxY) maxY = y;
                        if(y < minY) minY = y;
                        wallColumn.push_back(0);
                    }
                    else {
                        column.push_back(-1);
                        vantageColumn.push_back(-1);
                        wallColumn.push_back(0);
                    }
                }
                frontierColumn.push_back(false);
            }
            mapArray.push_back(column);
            vantageArray.push_back(vantageColumn);
            frontierArray.push_back(frontierColumn);
            wallArray.push_back(wallColumn);
        }

        //Set up the response as an occupancy grid

        response.frontier.info = map.info;
        response.frontier.header = map.header;

        response.frontier.data.resize(map.data.size());
        for(unsigned long int i=0; i<sizeof(response.frontier.data); i++) {
            response.frontier.data[i] = -1;
        }

        //Find the robot's square

        ROS_INFO("Robot pose in request: %f, %f", req.robotPose.position.x, req.robotPose.position.y);

        unsigned int robotX = floor((req.robotPose.position.x - map.info.origin.position.x) / map.info.resolution);
        unsigned int robotY = floor((req.robotPose.position.y - map.info.origin.position.y) / map.info.resolution);

        ROS_INFO("Robot square: %d, %d", robotX, robotY);

        mapArray[robotX][robotY] = 1;

        //Ignore the very edges of the map.

        if(maxX == mapArray.size()-1) maxX--;
        if(minX == 0) minX++;
        if(mapArray.size() > 0 && maxY == mapArray[0].size()-1) maxY--;
        if(minY == 0) minY++;

        //Start the waves.

        long int changed = 1;
        long int totalFrontierPoints = 0;
        int wave = 0;

        ROS_INFO("Examined map section: (%d, %d) to (%d, %d)", minX, minY, maxX, maxY);

        std::cout << "waves: " << std::endl;
        while(changed > 0) {
            wave++;
            changed = 0;
            for(unsigned int x=minX; x<maxX; x++) {
                for(unsigned int y=minY; y<maxY; y++) {
                    if(mapArray[x][y] == 0) {
                        for(unsigned int nx = x-1; nx <= x+1; nx++) {
                            for(unsigned int ny = y-1;ny <= y+1; ny++) {
                                if(mapArray[nx][ny] == -1 && !frontierArray[x][y]) {
                                    response.frontier.data[y * response.frontier.info.width + x] = 100;
                                    frontierArray[x][y] = true;
                                    changed++;
                                }
                                //if(mapArray[nx][ny] > mapArray[x][y] && mapArray[nx][ny] != INT_MAX && !frontierArray[x][y]) {
                                //mapArray[x][y] = mapArray[nx][ny] + 1;
                                //changed++;
                                //}
                            }
                        }
                    }
                }
            }
            std::cout << "wave " << wave << ": " << changed << "changed." << std::endl;
            totalFrontierPoints += changed;
        }

        // Get the best vantage points:
        // Apply the laser masks,
        // find the local maximums.


        //Apply masks

        std::cout << "Applying laser masks..." << std::endl;

        int done = 0;

        for(unsigned int x=minX; x < maxX && done <= totalFrontierPoints; x++) {
            for(unsigned int y=minY; y < maxY && done <= totalFrontierPoints; y++) {
                if(frontierArray[x][y]) {
                    unsigned int maskMinX = 0;
                    unsigned int maskMinY = 0;
                    unsigned int maskMaxX = laserMaskSize;
                    unsigned int maskMaxY = laserMaskSize;
                    if(x - laserMaskOffset < minX) maskMinX = laserMaskOffset - (x-minX);
                    if(y - laserMaskOffset < minY) maskMinY = laserMaskOffset - (y-minY);
                    if(x + laserMaskOffset > maxX) maskMaxX = laserMaskOffset - (maxX - x);
                    if(y + laserMaskOffset > maxY) maskMaxY = laserMaskOffset - (maxY - y);

					//Create the shaded mask
                    std::vector<std::deque<bool> > shadedMask = maskForPoint(&laserMask, laserMaskOffset, &wallArray, x, y);

					//Apply the mask itself
                    for(unsigned int maskX = maskMinX; maskX < maskMaxX; maskX++) {
                        unsigned int mapX = x - laserMaskOffset + maskX;
                        for(unsigned int maskY = maskMinY; maskY < maskMaxY; maskY++) {
                            unsigned int mapY = y - laserMaskOffset + maskY;
                            if(shadedMask[maskX][maskY] && vantageArray[mapX][mapY] >= 0) { // shadedMask changed from laserMask!
                                vantageArray[mapX][mapY]++;
                            }
                        }
                    }
                    done++;
                    if((done % (totalFrontierPoints / 10)) == 0) {
                        std::cout << "Done: " << (done / (totalFrontierPoints / 10))*10 << "%" << std::endl;
                    }
                }

            }
        }

        // Exclude the points too close to the walls

        int wallRange = floor(laserMinRange / map.info.resolution);

        //Initialize a RNG

        srand(time(NULL));

        int straightChance = 100; //(100%)
        int diagonalChance = 60; //(60%)

        //Start circular waves.
        //Statistically, this would be a close approximation

        for(int i=0; i<wallRange; i++) {

            for(int x=minX; x<maxX; x++) {
                for(int y=minY; y<maxY; y++) {
                    if(wallArray[x][y] == 0) {
                        int prop = rand() % 100;
                        for(int dx = -1; dx<=1 && wallArray[x][y] == 0; dx++) {
                            for(int dy = -1; dy<=1 && wallArray[x][y] == 0; dy++) {
                                if(dx != 0 || dy != 0) {
                                    int neighValue = wallArray[x+dx][y+dy];
                                    if(neighValue == i+1) {
                                        if((dx == 0 || dy == 0) && (prop <= straightChance)) {
                                            wallArray[x][y] = i+2;
                                        }
                                        else if(prop <= diagonalChance) {
                                            wallArray[x][y] = i+2;
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
            }

        }

        //Now exclude those close to walls

        int total = 0;
        int excluded = 0;

        for(unsigned int x=minX; x<maxX; x++) {
            for(unsigned int y=minY; y<maxY; y++) {
                total++;
                if(wallArray[x][y] > 0) {
                    vantageArray[x][y] = -1;
                    excluded++;
                }
            }
        }

        std::cout << "Excluded " << excluded << " out of " << total << std::endl;

        //Find local maximums (and global maximum)

        std::vector<unsigned int> lmx; //Local max X
        std::vector<unsigned int> lmy; //Local max Y
        std::vector<unsigned int> lmv; //Local max value

        int gmax = 0;

        std::cout << "Finding local maximums." << std::endl;

        long int iterationsPredicted = (maxX - minX) * (maxY - minY);
        long int iteration = 0;

        for(unsigned int x=minX; x<maxX; x++) {
            for(unsigned int y=minY; y<maxY; y++) {
                if(vantageArray[x][y] > 0) {
                    bool isLocalMax = true;
                    for(unsigned int nx=x-1; isLocalMax && nx<=x+1; nx++) {
                        for(unsigned int ny = y-1; isLocalMax && ny<=y+1; ny++) {
                            if(x!=nx || y!=ny) isLocalMax = (isLocalMax && (vantageArray[x][y] > vantageArray[nx][ny]));
                        }
                    }

                    if(isLocalMax) {
                        lmx.push_back(x);
                        lmy.push_back(y);
                        lmv.push_back(vantageArray[x][y]);
                        if(vantageArray[x][y] > gmax) gmax = vantageArray[x][y];
                    }
                }
                iteration++;
                if(iteration % (iterationsPredicted / 10) == 0) {
                    std::cout << "Done: " << (iteration / (iterationsPredicted / 10))*10 << "%" << std::endl;
                }
            }
        }

        std::cout << "Global maximum: " << gmax << std::endl;
        std::cout << "Vantage points: " << lmv.size() << std::endl;

        double nFactor = (double)gmax / 100.0;

		//Write the vantage points into the array and on the occupancy grids
		
        response.vantagePoints.vantagePoints.resize(lmx.size());

        for(unsigned int i = 0; i<lmx.size(); i++) {
            response.vantagePoints.vantagePoints[i].x = lmx[i];
            response.vantagePoints.vantagePoints[i].y = lmy[i];
            response.vantagePoints.vantagePoints[i].value = lmv[i];

            vantageMap.data[lmy[i] * vantageMap.info.width + lmx[i]] = 100;
        }

        response.frontier.info.origin.position.z = 1;
        vantageMap.info.origin.position.z = 2;
        //^Hacks so they could be actually seen in RViz...

        std::cout << "Publishing maps..." << std::endl;

        vantagePointArrayPub.publish(response.vantagePoints);
        frontierPub.publish(response.frontier);
        vantagePointPub.publish(vantageMap);

        std::cout << "Finished." << std::endl;

        return true;
    }

    void spin() {
        ros::Rate rate(2);
        while(ros::ok()) {
            ros::spinOnce();
            rate.sleep();
        }
    }

	//This class stores an occluded section (start X, Y, end X, y, starting angle and ending angle)
    class OccludedSection {
    public:
        int startX;
        int startY;
        int endX;
        int endY;
        double startAngle;
        double endAngle;

        OccludedSection(int startX=0, int startY=0, int endX=0, int endY=0, double startAngle=0, double endAngle=0) {
            this->startX = startX;
            this->startY = startY;
            this->endX = endX;
            this->endY = endY;
            this->startAngle = startAngle;
            this->endAngle = endAngle;
        }

        bool contains(double angle) {
            return (angle >= startAngle && angle <= endAngle);
        }
    };

	//Create the shaded mask
    std::vector<std::deque<bool> > maskForPoint (std::vector<std::deque<bool> > * unshadedMask,
                                                 int maskOffset,
                                                 std::vector<std::vector<int> > * walls,
                                                 int cx,
                                                 int cy) {

        std::vector<OccludedSection> occludedSections;

        std::vector<std::deque<bool> > scanned;

        std::vector<std::deque<bool> > output;

        for(int x=0; x<unshadedMask->size(); x++) {
            std::deque<bool> scannedCol;
            std::deque<bool> outputCol;
            for(int y=0; y<unshadedMask->at(x).size(); y++) {
                scannedCol.push_back(false);
                outputCol.push_back(true);
            }

            scanned.push_back(scannedCol);
            output.push_back(outputCol);
        }

		//Detect the wall sections
        for(int x=0; x<unshadedMask->size(); x++) {
            for(int y=0; y<unshadedMask->at(x).size(); y++) {
                if(!scanned[x][y]) {
                    int wallX = x+cx-maskOffset;
                    int wallY = y+cy-maskOffset;

                    scanned[x][y] = true;

                    if((*walls)[wallX][wallY] > 0) {

                        //Find the minimal angle of the occluded section

                        int cwSector = -1;
                        int ccwSector = -1;

                        //Sectors:
                        // 0: y>maskOffset, y>=x
                        // 1: x<maskOffset, x<=y
                        // 2: y<maskOffset, y<=x
                        // 3: x>maskOffset, x>=y

                        bool continuesCW = true;

                        int cwx = x;
                        int cwy = y;

                        std::vector<std::pair<int, int> > cwLookupOffsets;

                        do {

                            int newCwSector = 0;

                            if(cwy > maskOffset && cwy>=cwx) {
                                newCwSector = 0;
                            }
                            else if(cwx < maskOffset && cwx <= cwy) {
                                newCwSector = 1;
                            }
                            else if(cwy < maskOffset && cwy <= cwx) {
                                newCwSector = 2;
                            }
                            else {
                                newCwSector = 3;
                            }

                            if(newCwSector != cwSector) {
                                cwSector = newCwSector;
                                switch(cwSector) {
                                case 0:
                                {
                                    cwLookupOffsets.push_back(std::make_pair(1, -1)); //Bottom right
                                    cwLookupOffsets.push_back(std::make_pair(1, 0)); // Center right
                                    cwLookupOffsets.push_back(std::make_pair(1, 1)); // Top right
                                    cwLookupOffsets.push_back(std::make_pair(0, -1)); // Bottom center
                                    //^These ones actually increase the occluded angle
                                    cwLookupOffsets.push_back(std::make_pair(0, 1)); // Top center
                                    //^This one just in case the area continues
                                    break;
                                }
                                case 1:
                                {
                                    cwLookupOffsets.push_back(std::make_pair(1, 1)); // Top right
                                    cwLookupOffsets.push_back(std::make_pair(0, 1)); // Top center
                                    cwLookupOffsets.push_back(std::make_pair(-1, 1)); // Top left
                                    cwLookupOffsets.push_back(std::make_pair(1, 0)); // Center right
                                    //^Increase angle
                                    cwLookupOffsets.push_back(std::make_pair(-1, 0)); // Center left
                                    //^Just in case
                                    break;
                                }
                                case 2:
                                {
                                    cwLookupOffsets.push_back(std::make_pair(-1, 1)); //Top left
                                    cwLookupOffsets.push_back(std::make_pair(-1, 0)); // Center left
                                    cwLookupOffsets.push_back(std::make_pair(-1, -1)); // Bottom left
                                    cwLookupOffsets.push_back(std::make_pair(0, 1)); // Top center
                                    //^These ones actually increase the occluded angle
                                    cwLookupOffsets.push_back(std::make_pair(0, -1)); // Bottom center
                                    //^This one just in case the area continues
                                    break;

                                }
                                case 3:
                                {
                                    cwLookupOffsets.push_back(std::make_pair(-1, -1)); // Bottom left
                                    cwLookupOffsets.push_back(std::make_pair(0, -1)); // Bottom center
                                    cwLookupOffsets.push_back(std::make_pair(1, -1)); // Bottom right
                                    cwLookupOffsets.push_back(std::make_pair(-1, 0)); // Center left
                                    //^Increase angle
                                    cwLookupOffsets.push_back(std::make_pair(1, 0)); // Center right
                                    //^Just in case
                                    break;
                                }
                                }
                            }

                            continuesCW = false;

                            for(int i=0; i<cwLookupOffsets.size() && !continuesCW; i++) {

                                int scanX = cwx+cwLookupOffsets[i].first;
                                int scanY = cwy+cwLookupOffsets[i].second;

                                if(scanX >= 0 && scanY >= 0 && scanX < scanned.size() && scanY < scanned.size() &&
                                        !scanned[scanX][scanY] &&
                                        (*walls)[cwx+cwLookupOffsets[i].first + cx - maskOffset][cwy+cwLookupOffsets[i].second + cy - maskOffset] > 0) {
                                    cwx += cwLookupOffsets[i].first;
                                    cwy += cwLookupOffsets[i].second;
                                    continuesCW = true;
                                    scanned[cwx][cwy] = true;
                                    output[cwx][cwy] = false;
                                }
                            }

                        } while(continuesCW);

                        bool continuesCcW = true;

                        int ccwx = x;
                        int ccwy = y;

                        std::vector<std::pair<int, int> > ccwLookupOffsets;

                        do {

                            int newCcwSector = 0;

                            if(ccwy > maskOffset && ccwy>=ccwx) {
                                newCcwSector = 0;
                            }
                            else if(ccwx < maskOffset && ccwx <= ccwy) {
                                newCcwSector = 1;
                            }
                            else if(ccwy < maskOffset && ccwy <= ccwx) {
                                newCcwSector = 2;
                            }
                            else {
                                newCcwSector = 3;
                            }

                            if(newCcwSector != ccwSector) {
                                ccwSector = newCcwSector;
                                switch(ccwSector) {
                                case 0:
                                {
                                    ccwLookupOffsets.push_back(std::make_pair(-1, -1)); //Bottom left
                                    ccwLookupOffsets.push_back(std::make_pair(-1, 0)); // Center left
                                    ccwLookupOffsets.push_back(std::make_pair(-1, 1)); // Top left
                                    ccwLookupOffsets.push_back(std::make_pair(0, -1)); // Bottom center
                                    //^These ones actually increase the occluded angle
                                    ccwLookupOffsets.push_back(std::make_pair(0, 1)); // Top center
                                    //^This one just in case the area continues
                                    break;
                                }
                                case 1:
                                {
                                    ccwLookupOffsets.push_back(std::make_pair(1, -1)); // Bottom right
                                    ccwLookupOffsets.push_back(std::make_pair(0, -1)); // Bottom center
                                    ccwLookupOffsets.push_back(std::make_pair(-1, -1)); // Bottom left
                                    ccwLookupOffsets.push_back(std::make_pair(1, 0)); // Center right
                                    //^Increase angle
                                    ccwLookupOffsets.push_back(std::make_pair(-1, 0)); // Center left
                                    //^Just in case
                                    break;
                                }
                                case 2:
                                {
                                    ccwLookupOffsets.push_back(std::make_pair(1, 1)); //Top right
                                    ccwLookupOffsets.push_back(std::make_pair(1, 0)); // Center right
                                    ccwLookupOffsets.push_back(std::make_pair(1, -1)); // Bottom right
                                    ccwLookupOffsets.push_back(std::make_pair(0, 1)); // Top center
                                    //^These ones actually increase the occluded angle
                                    ccwLookupOffsets.push_back(std::make_pair(0, -1)); // Bottom center
                                    //^This one just in case the area continues
                                    break;

                                }
                                case 3:
                                {
                                    ccwLookupOffsets.push_back(std::make_pair(-1, 1)); // Top left
                                    ccwLookupOffsets.push_back(std::make_pair(0, 1)); // Top center
                                    ccwLookupOffsets.push_back(std::make_pair(1, 1)); // Top right
                                    ccwLookupOffsets.push_back(std::make_pair(-1, 0)); // Center left
                                    //^Increase angle
                                    ccwLookupOffsets.push_back(std::make_pair(1, 0)); // Center right
                                    //^Just in case
                                    break;
                                }
                                }
                            }

                            continuesCcW = false;

                            for(int i=0; i<ccwLookupOffsets.size() && !continuesCcW; i++) {
                                int scanX = ccwx+ccwLookupOffsets[i].first;
                                int scanY = ccwy+ccwLookupOffsets[i].second;

                                if(scanX >= 0 && scanY >= 0 && scanX < scanned.size() && scanY < scanned.size()
                                        && !scanned[ccwx+ccwLookupOffsets[i].first][ccwy+ccwLookupOffsets[i].second] &&
                                        (*walls)[ccwx+ccwLookupOffsets[i].first + cx - maskOffset][ccwy+ccwLookupOffsets[i].second + cy - maskOffset] > 0) {
                                    ccwx += ccwLookupOffsets[i].first;
                                    ccwy += ccwLookupOffsets[i].second;
                                    continuesCcW = true;
                                    scanned[ccwx][ccwy] = true;
                                    output[ccwx][ccwy] = false;
                                }
                            }

                        } while(continuesCcW);

                        OccludedSection os (cwx, cwy, ccwx, ccwy, 0, 0);

                        occludedSections.push_back(os);

                    }
                }
            }
        }

        //Occluded sections calculated

        //Draw the lines from the edges of the occlusions to the edges of the mask

        for(int i=0; i<occludedSections.size(); i++) {
            OccludedSection os = occludedSections.at(i);
            double dx = os.startX - maskOffset;
            double dy = os.startY - maskOffset;
            double length = sqrtf(dx*dx+dy*dy);
            dx/=length;
            dy/=length;

            if(dx == 0 && dy == 0) {
                dx = dy = 1;
            }

            double cx = os.startX;
            double cy = os.startY;

            int rcx = round(cx);
            int rcy = round(cy);

            do {
                output[rcx][rcy] = false;
                cx+=dx;
                cy+=dy;
                rcx = round(cx);
                rcy = round(cy);
            } while(rcx >= 0 && rcx < output.size() && rcy >= 0 && rcy < output.size());

            dx = os.endX - maskOffset;
            dy = os.endY - maskOffset;
            length = sqrtf(dx*dx+dy*dy);
            dx/=length;
            dy/=length;

            if(dx == 0 && dy == 0) {
                dx = dy = 1;
            }

            cx = os.endX;
            cy = os.endY;

            rcx = round(cx);
            rcy = round(cy);

            do {
                output[rcx][rcy] = false;
                cx+=dx;
                cy+=dy;
                rcx = round(cx);
                rcy = round(cy);
            } while(rcx >= 0 && rcx < output.size() && rcy >= 0 && rcy < output.size());
        }

        // Create another map that contains only the unoccluded component of the "output"

        std::vector<std::deque<bool> > selection;

        for(int x=0; x<output.size(); x++) {
            std::deque<bool> selColumn;
            for(int y=0; y<output.size(); y++) {
                selColumn.push_back(false);
            }
            selection.push_back(selColumn);
        }

		//Select the visible part starting from the center
        floodSelect(output, maskOffset, maskOffset, selection, selection.size());

		//Output should be the selected part, masked with the laser mask
        for(int x=0; x<output.size(); x++) {
            for(int y=0; y<output.size(); y++) {
                output[x][y] = (*unshadedMask)[x][y] && selection[x][y];
            }
        }


        return output;
    }

	//A simple iterative flood select implementation
    void floodSelect(std::vector<std::deque<bool> >& source, int x, int y, std::vector<std::deque<bool> >& destination, int dSize) {
        bool changed = false;
        destination[x][y] = true;
        int minX = x-1;
        int minY = y-1;
        int maxX = x+1;
        int maxY = y+1;
        do {
            changed = false;
            int newMinX = minX;
            int newMinY = minY;
            int newMaxX = maxX;
            int newMaxY = maxY;
            for(int cx = minX; cx < maxX; cx++) {
                for(int cy = minY; cy < maxY; cy++) {
                    if(!destination[cx][cy]) {
                        for(int dx = (cx == 0 ? 1 : -1); !destination[cx][cy] && dx <= (cx == dSize ? 0 : 1); dx++) {
                            if(source[cx+dx][cy]) {
                                destination[cx][cy] = true;
                                changed = true;
                                if(minX > 0 && cx == minX) {
                                    newMinX = minX - 1;
                                }
                                if(maxX < dSize-1 && cx == maxX) {
                                    newMaxX = maxX + 1;
                                }
                            }
                        }
                        for(int dy = (cy == 0 ? 1 : -1); !destination[cx][cy] && dy <= (cy == dSize ? 0 : 1); dy++) {
                            if(source[cx][cy+dy]) {
                                destination[cx][cy] = true;
                                changed = true;
                                if(minY > 0 && cy == minY) {
                                    newMinY = minY - 1;
                                }
                                if(maxY < dSize-1 && cy == maxY) {
                                    newMaxY = maxY + 1;
                                }
                            }
                        }
                    }
                }
            }

            minX = newMinX;
            maxX = newMaxX;
            minY = newMinY;
            maxY = newMaxY;

        }while(changed);
    }

    std::pair<double, double> calculateOcclusion(int x, int y, int centerOffset, double resolution) {
        std::pair<double, double> output;

        int quadrant = 0; //1, 2, 3, 4: geometric quadrants, 5: x=offset, y>offset, 6: x<offset, y=offset, 7: x=offset, y<offset, 8: x>offset, y=offset, 9: x=y=offset, won't happen.

        if(x > centerOffset) {
            if(y > centerOffset) {
                quadrant = 1;
            }
            else if(y < centerOffset) {
                quadrant = 4;
            }
            else {
                quadrant = 8;
            }
        }
        else if(x < centerOffset) {
            if(y > centerOffset) {
                quadrant = 2;
            }
            else if(y < centerOffset) {
                quadrant = 3;
            }
            else {
                quadrant = 6;
            }
        }
        else {
            if(y > centerOffset) {
                quadrant = 5;
            }
            else if(y < centerOffset) {
                quadrant = 7;
            }
            else {
                quadrant = 9;
            }
        }

        double cx = (double)centerOffset / resolution;
        double cy = cx;

        double occMin = 0;
        double occMax = 2*M_PI;

        double ominX = 0;
        double ominY = 0;
        double omaxX = 0;
        double omaxY = 0;

        double mx = x / resolution;
        double my = y / resolution;

        switch(quadrant) {
        case 1: {
            ominX = mx + resolution / 2.0;
            ominY = my - resolution / 2.0;
            omaxX = mx - resolution / 2.0;
            omaxY = my + resolution / 2.0;
            break;
        }
        case 2: {
            ominX = mx + resolution / 2.0;
            ominY = my + resolution / 2.0;
            omaxX = mx - resolution / 2.0;
            omaxY = my - resolution / 2.0;
            break;
        }
        case 3: {
            ominX = mx - resolution / 2.0;
            ominY = my + resolution / 2.0;
            omaxX = mx + resolution / 2.0;
            omaxY = my - resolution / 2.0;
            break;
        }
        case 4: {
            ominX = mx - resolution / 2.0;
            ominY = my - resolution / 2.0;
            omaxX = mx + resolution / 2.0;
            omaxY = my + resolution / 2.0;
            break;
        }
        case 5: {
            ominX = mx + resolution / 2.0;
            ominY = my - resolution / 2.0;
            omaxX = mx - resolution / 2.0;
            omaxY = my - resolution / 2.0;
            break;
        }
        case 6: {
            ominX = mx + resolution / 2.0;
            ominY = my - resolution / 2.0;
            omaxX = mx + resolution / 2.0;
            omaxY = my + resolution / 2.0;
            break;
        }
        case 7: {
            ominX = mx - resolution / 2.0;
            ominY = my + resolution / 2.0;
            omaxX = mx + resolution / 2.0;
            omaxY = my + resolution / 2.0;
            break;
        }
        case 8: {
            ominX = mx - resolution / 2.0;
            ominY = my - resolution / 2.0;
            omaxX = mx - resolution / 2.0;
            omaxY = my + resolution / 2.0;
            break;
        }

        default: {

            break;
        }
        }

        ominX -= cx;
        ominY -= cy;
        omaxX -= cx;
        omaxY -= cy;

        occMin = atan2(ominY, ominX);
        occMax = atan2(omaxY, omaxX);

        if(quadrant == 9) {
            occMin = 0;
            occMax = 2*M_PI;
        }

        output.first = occMin;
        output.second = occMax;
        return output;
    }

    ros::ServiceServer scanServer; //Receive requests for scans

protected:
    ros::Publisher frontierPub; //Publishes the frontier cells as a map
    ros::Publisher vantagePointPub; //Publishes the "vantage points" as an OccupancyGrid
    ros::Publisher vantagePointArrayPub; //Publishes the vantage points as an array of Vantage Points.


    ros::ServiceClient mapClient;
    ros::Subscriber mapSubscriber;

    nav_msgs::OccupancyGrid map;

    static const double laserRange = 3.00;
    static const double laserMinRange = 0.5;
    std::vector<std::deque<bool> > laserMask;
    int laserMaskSize;
    int laserMaskOffset;
};

int main(int argc, char **argv) {
    ros::init(argc, argv, "frontier"); // Initiate new ROS node named "frontier"
    ros::NodeHandle n;
    FrontierDetector detector(n); // Create new FrontierDetector object
    detector.scanServer = n.advertiseService("scan_frontier", &FrontierDetector::scanFrontiers, &detector);
    detector.spin(); // Execute main loop
    return 0;
}
